#include "stdafx.h"

#include "LocatorPoseMex.h"

void runCorrectDistortion(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {

	if (nrhs != 16) mexErrMsgTxt("CorrectDistortion needs 15 arguments.");
	if ( (mxGetM(prhs[1]) != 2) || (mxGetN(prhs[2]) != 1)) mexErrMsgTxt("center must be a 2x1 vector.");

	double x, y, a1, a2, a3, b1, b2, c1, c2, ppx, ppy, r0, ssx, ssy, esx, esy;
	//x = y = a1 = a2 = a3 = b1 = b2 = c1 = c2 = ppx = ppy = r0 = ssx = ssy = esx = esy = 0;
	
	memcpy(&x,   mxGetPr(prhs[1]),  sizeof(double));
	memcpy(&y,   mxGetPr(prhs[1])+1,  sizeof(double));
	memcpy(&a1,  mxGetPr(prhs[2]),  sizeof(double));
	memcpy(&a2,  mxGetPr(prhs[3]),  sizeof(double));
	memcpy(&a3,  mxGetPr(prhs[4]),  sizeof(double));
	memcpy(&b1,  mxGetPr(prhs[5]),  sizeof(double));
	memcpy(&b2,  mxGetPr(prhs[6]),  sizeof(double));
	memcpy(&c1,  mxGetPr(prhs[7]),  sizeof(double));
	memcpy(&c2,  mxGetPr(prhs[8]),  sizeof(double));
	memcpy(&ppx, mxGetPr(prhs[9]), sizeof(double));
	memcpy(&ppy, mxGetPr(prhs[10]), sizeof(double));
	memcpy(&r0,  mxGetPr(prhs[11]), sizeof(double));
	memcpy(&ssx,  mxGetPr(prhs[12]), sizeof(double));
	memcpy(&ssy,  mxGetPr(prhs[13]), sizeof(double));
	memcpy(&esx,  mxGetPr(prhs[14]), sizeof(double));
	memcpy(&esy,  mxGetPr(prhs[15]), sizeof(double));
	
	//mexPrintf("%f %f %f %f %f %f %f %f %f %f %f %f\n", x, y, a1, a2, a3, b1, b2, c1, c2, ppx, ppy, r0);

	OTPF::Marker::correctDistortion(	x,	// x-Koordinate
										y,	// y-Koordinate
										a1, a2, a3, // Rest: Verzeichnungsparameter
										b1,	b2, 
										c1, c2, 
										ppx, ppy, 
										r0,
										ssx, ssy,
										esx, esy
									);
									
	//mexPrintf("%f %f\n", x,y);
	plhs[0] = mxCreateDoubleMatrix(1, 2, mxREAL);
	//plhs[1] = mxCreateDoubleMatrix(1, 1, mxREAL);
	
	memcpy(mxGetPr(plhs[0]), &x, sizeof(double));
	memcpy(mxGetPr(plhs[0])+1, &y, sizeof(double));
	

}